/* ----------------------------------------------------------------------- */
/* Plantilla generada por Piklab */
/*
La configuracion del micro nos permite trabajar un con XT de 20MHZ
y pero usar el PLL con el propósito de alcanzar los 12MIPS

Códigos de mensajes de error
0 -- Buffer lleno   Se han enviado demasiados caracteres
1 -- turnPLus error  no puedo mover servo seleccionado
2 -- turnMinus error no puedo mover servo seleccionado
3 -- commandProcess error o puedo elegir ese servo

ok -- Proceso terminado correctamente
*/
#include <pic18f2550.h>
#include <delay.h>
#include <uart18f2550.c>
#include <stdlib.h>
#include <string.h>
/* ----------------------------------------------------------------------- */
/* Bits de configuración: adapte los parámetros a su necesidad */
code char at __CONFIG1L CONFIG1L = _PLLDIV_DIVIDE_BY_5__20MHZ_INPUT__1L & _CPUDIV__OSC1_OSC2_SRC___1__96MHZ_PLL_SRC___2__1L & _USBPLL_CLOCK_SRC_FROM_96MHZ_PLL_2_1L;
code char at __CONFIG1H CONFIG1H = _OSC_HS__HS_PLL__USB_HS_1H & _FCMEN_ON_1H & _IESO_ON_1H;
code char at __CONFIG2L CONFIG2L = _PUT_ON_2L & _BODEN_ON_2L & _BODENV_4_2V_2L & _VREGEN_ON_2L;
code char at __CONFIG2H CONFIG2H = _WDT_DISABLED_CONTROLLED_2H & _WDTPS_1_32768_2H;
code char at __CONFIG3H CONFIG3H = _CCP2MUX_RC1_3H & _PBADEN_PORTB_4_0__CONFIGURED_AS_DIGITAL_I_O_ON_RESET_3H & _LPT1OSC_ON_3H & _MCLRE_MCLR_OFF_RE3_ON_3H;
code char at __CONFIG4L CONFIG4L = _STVR_ON_4L & _LVP_OFF_4L & _ENHCPU_OFF_4L & _BACKBUG_OFF_4L;
code char at __CONFIG5L CONFIG5L = _CP_0_OFF_5L & _CP_1_OFF_5L & _CP_2_OFF_5L & _CP_3_OFF_5L;
code char at __CONFIG5H CONFIG5H = _CPB_OFF_5H;
code char at __CONFIG6L CONFIG6L = _WRT_0_OFF_6L & _WRT_1_OFF_6L & _WRT_2_OFF_6L & _WRT_3_OFF_6L;
code char at __CONFIG6H CONFIG6H = _WRTC_OFF_6H & _WRTB_OFF_6H;
code char at __CONFIG7L CONFIG7L = _EBTR_0_OFF_7L & _EBTR_1_OFF_7L & _EBTR_2_OFF_7L & _EBTR_3_OFF_7L;
code char at __CONFIG7H CONFIG7H = _EBTRB_OFF_7H;

//Definiciones de servos
#define SERVO1 LATBbits.LATB0
#define SERVO2 LATBbits.LATB1
#define SERVO3 LATBbits.LATB2
#define SERVO4 LATBbits.LATB3
#define SERVO5 LATBbits.LATB4
#define SERVO6 LATBbits.LATB5
#define SERVO7 LATBbits.LATB6
#define SERVO8 LATBbits.LATB7

#define CSERVO1 TRISBbits.TRISB0
#define CSERVO2 TRISBbits.TRISB1
#define CSERVO3 TRISBbits.TRISB2
#define CSERVO4 TRISBbits.TRISB3
#define CSERVO5 TRISBbits.TRISB4
#define CSERVO6 TRISBbits.TRISB5
#define CSERVO7 TRISBbits.TRISB6
#define CSERVO8 TRISBbits.TRISB7

typedef enum{false = 0, true = 1} boolean;  //para definir variables booleanas

//boolean tuza = true;  este es solo un ejemplo
//Constantes para la definición de ventanas
const unsigned Ticks4WindowH = 0x75;
const unsigned Ticks4WindowL = 0x2F;
const unsigned Ticks4CenterH = 0x46;
const unsigned Ticks4CenterL = 0x4F;
const unsigned Ticks4MinimumH = 0x17;
const unsigned Ticks4MinimumL = 0x6F;
const unsigned Ticks4MaximumH = 0x6B;
const unsigned Ticks4MaximumL = 0xCF;
static unsigned int Ticks4NextInterruptH = 0xCF;
static unsigned int Ticks4NextInterruptL = 0x2C;


const unsigned Ticks4Window = 0x752F;
const unsigned Ticks4Minimum = 0x176F;
const unsigned Ticks4Center = 0x464F;
const unsigned Ticks4Maximum = 0x6BCF;
static unsigned int Ticks4NextInterrupt = 0xCF2C;
static unsigned int Servo_PWM[8];
static unsigned int tempom=0x0000;
static unsigned int tempo=0x0000;
static unsigned int tempol;
static unsigned int tempoh;

static unsigned int Servo_PWMH[8];
static unsigned int Servo_PWML[8];
static unsigned short Servo_Idx = 0;
 
boolean SERVO1_ON = true;
boolean SERVO2_ON = true;
boolean SERVO3_ON = true;
boolean SERVO4_ON = true;
boolean SERVO5_ON = true;
boolean SERVO6_ON = true;
boolean SERVO7_ON = true;
boolean SERVO8_ON = true;

//definicion de teclas
#define INTRO 0x0D
#define RETROCESO 0x08
#define ESCAPE 0x1B

boolean flag_Phase;
unsigned short j=0xFF;
unsigned int posI;

/*Seccion para la comunicacion en serial*/

#define  lenbuff 10

unsigned char xbuff=0x00;
unsigned char cbuff[lenbuff];
char rcvchar=0x00;
boolean flagcommand = false;

//variables para timers
	unsigned char CounterA;
	unsigned char CounterB;

//Prototipos
void init_board(void);
void EnableTimer1Interrupt(void);
void Tmr1Carga(unsigned char high, unsigned char low);
void initBuff(void);
void add2cbuff(unsigned char c);
void echosel(char c);
void command_process(void);
void delay_ms(int milis);
void delay_us(int milis);
unsigned int SelectServo(unsigned char Servo);
void turnPlus(int vueltas, unsigned char vel, unsigned char Servo);
void turnMinus(int vueltas, unsigned char vel,unsigned char Servo);

//Sección de interrupciones

static void isr_Tmr1() interrupt 1    //Esta es una interrupcion de alta prioridad
{
	PIE1bits.RCIE=0;  //deshabilitamos la interrupcion del puerto serie
	if(flag_Phase == 1)
	{
		if(Servo_Idx == 0 && SERVO1_ON) {SERVO1 = 1;}
		if(Servo_Idx == 1 && SERVO2_ON) {SERVO2 = 1;}
		if(Servo_Idx == 2 && SERVO3_ON) {SERVO3 = 1;}
		if(Servo_Idx == 3 && SERVO4_ON) {SERVO4 = 1;}
		if(Servo_Idx == 4 && SERVO5_ON) {SERVO5 = 1;}
		if(Servo_Idx == 5 && SERVO6_ON) {SERVO6 = 1;}
		if(Servo_Idx == 6 && SERVO7_ON) {SERVO7 = 1;}
		if(Servo_Idx == 7 && SERVO8_ON) {SERVO8 = 1;}
		Ticks4NextInterruptH = 0xFF - Servo_PWMH[Servo_Idx];
		Ticks4NextInterruptL = 0xFF - Servo_PWML[Servo_Idx];
		Tmr1Carga(Ticks4NextInterruptH,Ticks4NextInterruptL);
	}
	
	if(flag_Phase == 0)
	{
		if(Servo_Idx == 0 && SERVO1_ON) {SERVO1 = 0;}
		if(Servo_Idx == 1 && SERVO2_ON) {SERVO2 = 0;}
		if(Servo_Idx == 2 && SERVO3_ON) {SERVO3 = 0;}
		if(Servo_Idx == 3 && SERVO4_ON) {SERVO4 = 0;}
		if(Servo_Idx == 4 && SERVO5_ON) {SERVO5 = 0;}
		if(Servo_Idx == 5 && SERVO6_ON) {SERVO6 = 0;}
		if(Servo_Idx == 6 && SERVO7_ON) {SERVO7 = 0;}
		if(Servo_Idx == 7 && SERVO8_ON) {SERVO8 = 0;}
		Ticks4NextInterruptH = 0xFF - Ticks4WindowH;
		Ticks4NextInterruptL = 0xFF - Ticks4WindowL;
		
		if(Ticks4NextInterruptL + Servo_PWML[Servo_Idx] > 0xFF)
		{
			Ticks4NextInterruptL = Ticks4NextInterruptL + Servo_PWML[Servo_Idx];
			Ticks4NextInterruptH = Ticks4NextInterruptH + Servo_PWMH[Servo_Idx] + 1;
		}
		else
		{
			Ticks4NextInterruptL = Ticks4NextInterruptL + Servo_PWML[Servo_Idx];
			Ticks4NextInterruptH = Ticks4NextInterruptH + Servo_PWMH[Servo_Idx];
		}
		Tmr1Carga(Ticks4NextInterruptH, Ticks4NextInterruptL);
		if(++Servo_Idx > 7 ) Servo_Idx = 0;
	}
	if(++flag_Phase > 1) flag_Phase = 0;
	
	//Esta instruccion se esta ejecutando cada 13ms con un xtal de 20MHz	
    LATBbits.LATB7^=1; //complementamos el valor del bit RB7
    PIR1bits.TMR1IF=0; //limpiamos la bandera de interrupcion
    PIE1bits.RCIE=1;   //habilitamos la interrupcion del puerto serie
}


static void isr_Serial() interrupt 2  //Esta es una interrupcion de baja prioridad
{
	PIR1bits.RCIF=0;
	rcvchar=RCREG;
	add2cbuff(rcvchar);
	echosel(rcvchar);
}

//Funcion Principal
void main() {
//declaracion de variables
int i=0;
int button_down=1;
int valTimer=0;

RCONbits.IPEN=1; //interrupciones por prioridad habilitadas
init_board();
Print_str("Dispositivo inicializado");
Printf(INTRO);
Print_str("PIC18F2550 en SDCC");
Printf(INTRO);
delay_ms(500); //tiempo de espera
initBuff();

for (i=0;i<9;i++)
{
	Servo_PWMH[i] = Ticks4CenterH;
	Servo_PWML[i] = Ticks4CenterL;
}

EnableTimer1Interrupt();
tempo = Ticks4Center;
tempoh = tempo & 0xFF00;
tempol = tempo & 0x00FF;
Tmr1Carga(tempoh, tempol);
posI = Ticks4Center;

		


while (true)
{
	if(flagcommand)   //si ya presionaste enter
	{
	command_process();
	}
}
}

void init_board(void) {
	flagcommand= false;
ADCON1=0x7; // PORTA used for digital I/O
TRISAbits.TRISA4=true; // configure PORTA4 for input (pushbutton)

/* Configuramos las salidas de los servomotores*/
CSERVO1 = false;
CSERVO2 = false;
CSERVO3 = false;
CSERVO4 = false;
CSERVO5 = false;
CSERVO6 = false;
CSERVO7 = false;
CSERVO8 = false;

//Configuramos la comunicacion serial --rutina de jean Pierre Mandon
setup_UART(B57600);
}

void EnableTimer1Interrupt(void)
{
	T1CON=1;        //Timer1 preescaler 1 interno, carga de 8 bits
	PIE1bits.TMR1IE=1;
	INTCONbits.PEIE=1;
	INTCONbits.GIE=1;
}

void Tmr1Carga(unsigned char high, unsigned char low)
{
	TMR1H=high;
	TMR1L=low;
}
		
//Funcion de vaciado del buffer
void initBuff(void){
	int i;
	for (i=0; i< lenbuff;i++){
		cbuff[i]=0x0;
	}xbuff=0x00;
}

//agrega un caracter mas al buffer, o realiza un acción
void add2cbuff(unsigned char c)
{
	if(xbuff<lenbuff){  //Añade caracteres al buffer mientras no se llene el buffer
		switch(c){
			case INTRO: //Enter  --> Habilita flag para procesar comando
				flagcommand=true;
				break;
			case RETROCESO: //BackSpace --> Borra del buffer el ultimo caracter insertado
				if(xbuff>0) cbuff[--xbuff]=0x00;
				break;
			case ESCAPE: //ESC  --> Borra todo el buffer
				initBuff();
				break;
			default:  //añade caracter al buffer recibido
				cbuff[xbuff++]=c;
		}
	}
	else{
		initBuff();//Si el buffer esta lleno, vacialo
		Print_str("0");  //Envia mensaje de que el buffer esta lleno Error 0
		Printf(INTRO);
	}
}

//Solo imprime los caracteres imprimibles
void echosel(char c)
{
	switch(c){
		case 0x0D: //Si presionamos la tecla intro
     			Printf(INTRO);
			break;
		case 0x08: //Si presionamos la tecla BackSpace
			Printf(RETROCESO);
			break;
		case 0x1B: //Si presionamos la tecla ESC
			Printf(ESCAPE);
			break;
		default:
			Printf(c); //Echo de cualquier otro caracter
	}
}

/*
El formato de los comandos es el siguiente:
[a...f] [123] [xx]
servo posicion velocidad
primer byte un caracter alfabético de entre a y e que define al servo
tres bytes para definir la posicion del servo
dos bytes para definir la velocidad de movimiento del servo
a12301 {enter}
mueve el servo 1 a 123 grados de la posicion de origen a la velocidad 01
*/
void command_process()
{
	int i=0;
	char DerIzq= 1; //bandera para saber si va a 180 o a 0 grados
   // DerIzq = 1   Indicará que vamos de 0 a 180 por tanto suma
   // DerIzq = 0   Indicará que vamos de 180 a 0 por tanto resta
	unsigned char cmd[lenbuff]; //comando tecleado
	unsigned char servo[2];
	unsigned char pos[4];  //para almacenar las posiciones
	unsigned char posicion;
	unsigned char velocidad[4];  //para almacenar los incrementos
	unsigned char vel;
	
	unsigned int posF;  //Es posible que estos valores los tengas que dividir
	unsigned int vueltas;
	
	
	flagcommand=false;
	//Recibe el nombre del servo a mover
	strcpy(cmd,cbuff);  
	strncpy(servo,cmd,1);
	//guarda la posicion del servo
	strcpy(cmd,cbuff); 
	for(i=1;i<4;i++){
		pos[i-1]= cmd[i];
	}
	posicion = atoi(pos);
	//guarda la velocidad a la que se moverá el servo
	strcpy(cmd,cbuff);
	for(i=4;i<7;i++)
	{
		velocidad[i-4]=cmd[i];
	}
	vel = atoi(velocidad);
	tempo = 0;
	tempoh=0;
	tempol=0;
	switch(servo[0]){
		case 'a':
			posI=SelectServo(0);
			break;
		case 'b':
			posI=SelectServo(1);
			break;
		case 'c':
			posI=SelectServo(2);
			break;
		case 'd':
			posI=SelectServo(3);
			break;
		case 'e':
			posI=SelectServo(4);
			break;
		case 'f':
			posI=SelectServo(5);
			break;
		default:
			Print_str("3"); //Error 3 opcion elegida no válida
			Printf(INTRO);
			break;
	}
	posF= 106.66 * posicion + 8399;

	
	if(posF>posI){
		vueltas = posF - posI;
		vueltas = vueltas / vel;
		DerIzq = 1;
	}
	else
	{
		vueltas = posI - posF;
		vueltas = vueltas/ vel;
		DerIzq = 0;
	}
	
	/********************************************************/
	/*
	Debe de leerse el valor que tiene del servo desde el array
	Žpara determinar el valor inicial y no unicamente dejarlo
	como valor final. esto causa el eeror de que al mover
	multiples motores con una unica variable se moveran en conse-
	cuencia del anterior
	*/
  // posI = posF;  Esto no es correcto
   
	if(DerIzq)
	{
		switch(servo[0])
		{
			case 'a':  //mueve al servo uno en incrementos de vel hasta llegar a vueltas
				turnPlus(vueltas, vel, 0);
					break;
			case 'b':  //mueve al servo dos en incrementos de vel hasta llegar a vueltas
				turnPlus(vueltas, vel, 1);
					break;
			case 'c':

				
				turnPlus(vueltas, vel, 2);
					break;
			case 'd':

				turnPlus(vueltas, vel, 3);
					break;
			case 'e':

				turnPlus(vueltas, vel, 4);
					break;
			default:
				Print_str("1"); Printf(INTRO); //Error 1 turnPlusError
		}
	}
	else{
		switch(servo[0])
		{
			case 'a':  //mueve al servo uno en incrementos de vel hasta llegar a vueltas
				turnMinus(vueltas,vel,0);

					break;
			case 'b':  //mueve al servo dos en incrementos de vel hasta llegar a vueltas
				turnMinus(vueltas,vel,1);
					break;
			case 'c':
				turnMinus(vueltas,vel,2);
					break;
			case 'd':
				turnMinus(vueltas,vel,3);
					break;
			case 'e':
				turnMinus(vueltas,vel,4);
					break;
			default:
				Print_str("2");Printf(INTRO); // error 2 turnMinus Error
		}   
	}
	initBuff();
	Print_str("ok");Printf(INTRO);
}
		
void delay_ms(int milis){
delay1ktcy(10*milis);

}

void delay_us(int milis){
delay10tcy(1*milis);
}

unsigned int SelectServo(unsigned char Servo)
{
			tempoh = Servo_PWMH[Servo];
			tempo = tempoh << 8;
			return (tempo | Servo_PWML[Servo]);
}

void turnPlus(int vueltas, unsigned char vel, unsigned char Servo)
{
int i=0;

for(i=0;i<vueltas;i++)
				{
					tempoh = Servo_PWMH[Servo];
					tempoh = tempoh << 8;
					tempo  = tempoh | Servo_PWML[Servo];
					tempo += vel;
					Servo_PWML[Servo] = tempo & 0x00FF;
					tempoh = tempo & 0xFF00;
					tempoh = tempoh >> 8;
					Servo_PWMH[Servo] = tempoh;
					delay_us(100);
				}
}

void turnMinus(int vueltas, unsigned char vel, unsigned char Servo)
{
int i=0;

for(i=0;i<vueltas;i++)
				{
					tempoh = Servo_PWMH[Servo];
					tempoh = tempoh << 8;
					tempo  = tempoh | Servo_PWML[Servo];
					tempo -= vel;
					Servo_PWML[Servo] = tempo & 0x00FF;
					tempoh = tempo & 0xFF00;
					tempoh = tempoh >> 8;
					Servo_PWMH[Servo] = tempoh;
					delay_us(100);
				}
}
